#!/usr/bin/env python

import rospy
from gazebo_msgs.srv import SetModelState
from gazebo_msgs.msg import ModelState

def set_robot_height(model_name, target_height):
    rospy.wait_for_service('/gazebo/set_model_state')
    try:
        set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
        state_msg = ModelState()
        state_msg.model_name = model_name
        state_msg.pose.position.z = target_height
        state_msg.pose.position.x = 0
        state_msg.pose.position.y = 0
        state_msg.pose.orientation.x = 0
        state_msg.pose.orientation.y = 0
        state_msg.pose.orientation.z = 0
        state_msg.pose.orientation.w = 1
        state_msg.reference_frame = 'world'
        resp = set_state(state_msg)
        return resp
    except rospy.ServiceException as e:
        rospy.logerr("Service call failed: %s" % e)

def hover_control():
    rospy.init_node('hover_control', anonymous=True)
    rate = rospy.Rate(200)  # 10 Hz
    model_name = "aliengo_gazebo"
    target_height = rospy.get_param('~target_height', 1.0)
    
    while not rospy.is_shutdown():
        set_robot_height(model_name, target_height)
        rate.sleep()

if __name__ == '__main__':
    try:
        hover_control()
    except rospy.ROSInterruptException:
        pass
